
#include <QtWidgets>
#include "PelcoD.h"

PelcoD::PelcoD()
{
    zoom_limit_ = 65535;
}

PelcoD::~PelcoD()
{
}

std::string PelcoD::GetData(const ProtocolData&data)
{
    std::string temp;

    temp.append(1, data.Byte1_);
    temp.append(1, data.Address_);
    temp.append(1, data.Command1_);
    temp.append(1, data.Command2_);
    temp.append(1, data.Data1_);
    temp.append(1, data.Data2_);
    temp.append(1, data.CheckSum_);

    return temp;
}

ProtocolData PelcoD::SetValue(PELCOD_COMMAND command, unsigned short val, unsigned char address)
{
    ProtocolData data;

    data.Byte1_ = 0xFF;
    data.Address_ = address;

    unsigned short st_command = (unsigned short)command;

    data.Command1_ = st_command >> 8;
    data.Command2_ = st_command & 0xff;

    data.Data1_ = val >> 8;
    data.Data2_ = val & 0xff;

    data.CheckSum_ = data.Address_ + data.Command1_ + data.Command2_ + data.Data1_ + data.Data2_;

    return data;
}

std::string PelcoD::GetProtocolStr(PELCOD_COMMAND command, unsigned short val, unsigned char address)
{
    return GetData(SetValue(command, val, address));
}

std::string PelcoD::Stop(unsigned int address)
{
    return GetProtocolStr(SET_STOP_COMMAND, 0, address);
}

std::string PelcoD::SetP(double p)
{
    if (p < 0 || p > 360.0)
    {
        return "";
    }

    unsigned short val = p * 100;

    return GetProtocolStr(SET_P_COMMAND, val);
}

std::string PelcoD::SetT(double t)
{
    unsigned short val = t * 100;

    return GetProtocolStr(SET_T_COMMAND, val);
}

std::string PelcoD::SetZV(double z)
{
    unsigned short val = (unsigned short)((z / zoom_limit_) * 65535);

    return GetProtocolStr(SET_Z_COMMAND, val, Z_VISIBLE);
}

std::string PelcoD::SetZN(double z)
{
    unsigned short val = (unsigned short)((z / zoom_limit_) * 65535);

    return GetProtocolStr(SET_Z_COMMAND, val, Z_NIGHT);
}

std::string PelcoD::SetFocus(unsigned short focus)
{
    return GetProtocolStr(SET_FOCUS_COMMAND, focus);
}

std::string PelcoD::QueryP()
{
    return GetProtocolStr(QUERY_P_COMMAND);
}

std::string PelcoD::QueryT()
{
    return GetProtocolStr(QUERY_T_COMMAND);
}

std::string PelcoD::QueryZ()
{
    return GetProtocolStr(QUERY_Z_COMMAND);
}

std::string PelcoD::QueryFocus()
{
    return GetProtocolStr(QUERY_FOCUS_COMMAND);
}

std::string PelcoD::PtzControl(PELCOD_COMMAND command, unsigned char horizontal_speed, unsigned char vertical_speed)
{
    unsigned short val;

    if (command == PTZ_MOVE_CTRL_CMD_MOVE_DOWN || command == PTZ_MOVE_CTRL_CMD_MOVE_UP)
    {
        val = vertical_speed;
    }
    else if (command == PTZ_MOVE_CTRL_CMD_MOVE_LEFT || command == PTZ_MOVE_CTRL_CMD_MOVE_RIGHT)
    {
        val = (unsigned short)(horizontal_speed << 8);
    }
    else
    {
        val = ((unsigned short)(horizontal_speed) << 8) | vertical_speed;
    }

    return GetData(SetValue(command, val));
}

std::string PelcoD::CameraControl(PELCOD_COMMAND command, unsigned char address)
{
    return GetProtocolStr(command, 0, address);
}